import lejos.nxt.TouchSensor;
import lejos.nxt.SensorPort;
import lejos.nxt.Motor;
import lejos.robotics.navigation.DifferentialPilot;
/**
 * Handles the movement of the robot.
 * 
 * @author sava
 *  
 */
public class Movement {

	private static DifferentialPilot pilot = new DifferentialPilot(5.6f,
			11.25f, Motor.A, Motor.B, false); // parameters in inches for
												// rotation
	private static TouchSensor ts = new TouchSensor(SensorPort.S4);

	/**
	 * Moves the robot cms cm. If cms is positive the robot moves forward, if
	 * negative it moves backward.
	 * 
	 * @param cms
	 * @return The number of cm actually travelled.
	 */
	public static int move(int cms) {

		pilot.setTravelSpeed(10.0f);
		pilot.travel(cms, true);
		System.out.println("req" + Integer.toString(cms));
		while (pilot.isMoving()) { // If we touch a wall while moving go back
									// 10cm
			
			if(USSensor.sensor.getDistance()<30){
				cms = (int) pilot.getMovementIncrement();
				pilot.stop();
			}
		}
		System.out.println("real" + Integer.toString(cms));
		return cms; // TODO: Return the correct value.
	}

	/**
	 * Rotates the robot degrees degrees. If degrees is positive the robot moves
	 * clockwise, otherwise counter-clockwise.
	 * 
	 * @param degrees
	 * @return The number of degrees actually turned.
	 */
	public static int rotate(int degrees) {
		pilot.setRotateSpeed(75.0f);
		pilot.rotate(degrees);
		return degrees; //TODO: Return the correct value.
	}

	/**
	 * Rotates the robot 90 degrees to the left.
	 */
	public static void rotateLeft() {
		Movement.rotate(-90);
	}

	/**
	 * Rotates the robot 90 degrees right.
	 */
	public static void rotateRight() {
		Movement.rotate(90);
	}

	/**
	 * Rotates the robot 180 degrees to face backwards from the original position.
	 */
	public static void rotateBack() {
		Movement.rotate(180);
	}
}
